#include <stdlib.h>
#include "avr_compiler.h"
#include "uart.h"
#include <stdio.h>

//Estop Limit Defines
#define DEFAULT_MAX_TEMP 50

//For Bus voltage divider, 249k and 10k, should probably change if you want to
//use software testing board
#define DEFAULT_VOLTAGE_UPPER_LIMIT 42161 //55V
#define DEFAULT_VOLTAGE_LOWER_LIMIT 20000 //~27V

#define ESTOPPED 1

#define PHASE_CORRECTION_MODE_DEFAULT 0

#define MOTION_ERROR_LIMIT_DEFAULT 10000
#define SPI_COMMAND_DEFAULT 0
#define MOTOR_MODE_DEFAULT 0
#define COMMUTATION_MODE_DEFAULT 1
#define LOOP_MODE_DEFAULT 0
#define DEAD_TIME_DEFAULT 55
#define MOTOR_ARMED_DEFAULT 0
#define MOTOR_COMMAND_DEFAULT 0
#define CONDITION_MASK1_DEFAULT 2
#define CONDITION_MASK2_DEFAULT 0
#define MOTOR_LIMIT_DEFAULT 10000
#define PWM_SENSE_DEFAULT 0b00111111
#define SIGNAL_SENSE_DEFAULT 0//0x0380
//Number of counts per electrical cycle
//2*counts per rot/poles
#define PHASE_COUNTS_DEFAULT 683
#define VELOCITY_SCALAR_DEFAULT 500
#define MOTOR_ACCELLERATION_DEFAULT 256*(1<<9)
#define VELOCITY_LOOP_GAIN_KP_DEFAULT 40
#define VELOCITY_LOOP_GAIN_KI_DEFAULT 8
#define VELOCITY_LOOP_GAIN_LIMIT_DEFAULT 20

#define CURRENT_LOOP_GAIN_KP_DEFAULT 16
#define CURRENT_LOOP_GAIN_KI_DEFAULT 4
#define CURRENT_LOOP_GAIN_LIMIT_DEFAULT 40



typedef struct
{
	//UART module pmd is located on
	USART_data_t uart;
	volatile uint8_t data_in[35];
	volatile uint8_t data_in_count;
	volatile uint8_t data_available;
	volatile uint8_t motor_mode;
	volatile uint8_t commutation_mode;
	volatile uint8_t loop_mode;
	volatile uint8_t dead_time;
	volatile uint8_t motor_armed;
	volatile int16_t motor_command;
	volatile uint16_t condition_mask1;
	volatile uint16_t condition_mask2;
	volatile uint16_t motor_limit;
	volatile uint16_t pwm_sense;
	volatile uint16_t signal_sense;
	volatile uint8_t data_request_flag;
	volatile uint8_t data_read_request;
	volatile uint16_t phase_counts;
	volatile uint8_t serial_flag;
	volatile uint8_t serial_mode;
	volatile uint16_t velocity_scalar;
	volatile uint32_t motor_accelleration;
	volatile uint16_t velocity_loop_gain_kp;
	volatile uint16_t velocity_loop_gain_ki;
	volatile uint16_t velocity_loop_gain_limit;
	volatile uint16_t current_loop_gain_kp;
	volatile uint16_t current_loop_gain_ki;
	volatile uint16_t current_loop_gain_limit;
	volatile uint16_t spi_motor_command;
	int16_t current_A;
	int16_t current_B;

	int16_t current_command_A;
	int16_t current_command_B;

	int16_t error_A;
	int16_t error_B;

	volatile uint16_t bus_voltage;
	volatile int16_t temp;
	volatile uint8_t check_sum;
	volatile int32_t motor_velocity;
	volatile uint8_t command_source;
	volatile uint8_t status;

	uint8_t phase_correction_mode;


	//Limits and estop stuff
	uint16_t bus_voltage_upper_limit;
	uint16_t bus_voltage_lower_limit;
	int16_t temp_max;
	uint16_t event_status;
	
	uint16_t motion_error_limit;

} PC_Control_t;

void pc_init(PC_Control_t * pc, USART_t * uart, int baud, PMD_Control_t * pmd;
);
void pc_handle(PC_Control_t * pc);
void pc_set_pmd(PC_Control_t * pc, PMD_Control_t * pmd);
void send_pc_update(PC_Control_t * pc);
void pc_acknowledge(PC_Control_t * pc);

void pc_give_command(PC_Control_t * pc);
void pc_give_velocity(PC_Control_t * pc);
void pc_give_spi(PC_Control_t * pc);
void pc_give_current(PC_Control_t * pc);
void pc_give_error(PC_Control_t * pc);
void pc_give_event_status(PC_Control_t * pc);
void pc_give_current_command(PC_Control_t * pc);

